/**
 * @file regularMultigridSolver.h
 * @brief All the class needed for solving the Possion PDE by Finite element and multigrid solver.
 * @author XDDDD
 * @version 
 * @date 2021-06-19
 */

#ifndef __PAISLEYPARK__REGULARMULTIGRIDSOLVER_H__
#define __PAISLEYPARK__REGULARMULTIGRIDSOLVER_H__

#include "Mesh.h"
#include "Element.h"
#include <iostream>
#include <cmath>
#include <fstream>
#include <vector>
#include <lapacke.h>

class MultigridVector_2d : public std::vector<double> {
public:
	/**
	 * @brief Default construction
	 */
	MultigridVector_2d();
	/**
	 * @brief Construction
	 *
	 * @param _u The given initial guess vector.
	 * @param _Nx The given order of the elements on the x axis. The length of _u is 2^N.
	 * @param _Ny The given order of the elements on the y axis. The length of _u is 2^N.
	 */
	MultigridVector_2d(const std::vector<double>& _u, const size_t _Nx, const size_t _Ny);
	/**
	 * @brief Set the Vector.
	 *
	 * @param _u The given initial guess vector.
	 * @param _Nx The given order of the number of elements on the x axis.
	 * @param _Ny The given order of the number of elements on the y axis.
	 */
	void set(const std::vector<double>& _u, const size_t _Nx, const size_t _Ny);
	/**
	 * @brief Return the value of one knots.
	 *
	 * @param _coord The given coordinate of the knot. 
	 * @param _M The index of the grids.
	 *
	 * @return The value of the knot.
	 */
	const double& operator()(const std::vector<size_t>& _coord, const size_t _M) const;
	/**
	 * @brief Return the value of one knot.
	 *
	 * @param _coord The given coordinate of the knot. 
	 * @param _M The index of the grids.
	 *
	 * @return The value of the knot.
	 */
	double& operator()(const std::vector<size_t>& _coord, const size_t _M);
	/**
	 * @brief Return the value of one knot.
	 *
	 * @param index The index of the knot.
	 * @param _M The index of the grids.
	 *
	 * @return The value of the knot
	 */
	const double& operator()(const size_t index, const size_t _M) const;
	/**
	 * @brief Return the value of one knot.
	 *
	 * @param index The index of the knot.
	 * @param _M The index of the grids.
	 *
	 * @return The value of the knot
	 */
	double& operator()(const size_t index, const size_t _M);
	/**
	 * @brief Get the number of the grids
	 *
	 * @return  The number of the grids.
	 */
	const size_t get_N() const;
	/**
	 * @brief Get the order of x axis the finest grid.
	 *
	 * @return  The order of x axis of the finest grid.
	 */
	const size_t get_Nx() const;
	/**
	 * @brief Get the order of y axis the finest grid.
	 *
	 * @return  The order of y axis of the finest grid.
	 */
	const size_t get_Ny() const;
	/**
	 * @brief Get the start index of the grid.
	 *
	 * @param _M The index of the grids.
	 *
	 * @return The start index of the grid with order _M
	 */
	const size_t get_start_index(const size_t _M) const;
	/**
	 * @brief Set all the elements in one grid to zero.
	 *
	 * @param _M The index of the grids.
	 */
	void clean(const size_t _M);
	/**
	 * @brief Set all the elements except the boundary one in one grid to zero.
	 *
	 * @param _M The index of the grids.
	 */
	void cleanpart(const size_t _M);
private:
	/**
	 * @brief The order of the x axis of the finest grid.
	 */
	size_t __Nx;
	/**
	 * @brief The order of the y axis of the finest grid.
	 */
	size_t __Ny;
	/**
	 * @brief The vector to store the starting index of each grid.
	 */
	std::vector<double> __start_index;
};

class RestrictionOperator_2d {
public:
	/**
	 * @brief Restrict the given vector into the coarser grid. The boundary knots won't be influnced.
	 *
	 * @param _v The given vector, is stored in Multigrid vector, which contains all the knots of each grids.
	 * @param _M The grid we want to restrict from.
	 */
	virtual void operator()(MultigridVector_2d& _v, const size_t _M) = 0;
};

class InterpolationOperator_2d {
public:
	/**
	 * @brief interpolate the vector on coarse grid to finer one. The boundary of the grid won't be influnced.
	 *
	 * @param _v The given vector, which is multigrid vector, containing all the elements of each grids.
	 * @param _M The coarse grid we want to interpolate from.
	 */
	virtual void operator()(MultigridVector_2d& _v, const size_t _M) = 0;
};

class Restriction_2d : public RestrictionOperator_2d {
public:
	void operator()(MultigridVector_2d& _v, const size_t _M) override;
};

class Interpolation_2d : public InterpolationOperator_2d {
public:
	void operator()(MultigridVector_2d& _v, const size_t _M) override;
};

class MultigridSolver_2d {
public:
	/**
	 * @brief Construction
	 */
	MultigridSolver_2d();
	/**
	 * @brief Destruction
	 */
	~MultigridSolver_2d(){};
	/**
	 * @brief Set the grid.
	 *
	 * @param _v The given initial guess with the boundary condition.
	 * @param _Nx The order of the x axis on the finest grid.
	 * @param _Ny The order of the y axis on the finest grid.
	 * @param _f The RHS of the equation.
	 */
	void set(const std::vector<double>& _v, const size_t _Nx, const size_t _Ny, const std::vector<double>& _f);	
	/**
	 * @brief Omit the small elements.
	 *
	 * @param _epsilon The tolerence of the error.
	 */
	void omit_e(const double _epsilon);
	/**
	 * @brief Get the order of the finest grid.
	 *
	 * @return __N
	 */
	const size_t get_N() const;
	/**
	 * @brief Get the order of the x axis on the finest grid.
	 *
	 * @return __N
	 */
	const size_t get_Nx() const;
	/**
	 * @brief Get the order of the y axis on the finest grid.
	 *
	 * @return __N
	 */
	const size_t get_Ny() const;
	/**
	 * @brief Get the value of one knot of __U. 
	 *
	 * @param _coord The coord of the knot.
	 * @param _N The order of the grid.
	 *
	 * @return The value of the knot
	 */
	const double get_U(const std::vector<size_t>& _coord, const size_t _N) const;
	/**
	 * @brief Get the value of one knot of __F.
	 *
	 * @param _coord The coord of the knot.
	 * @param _N The order of the grid.
	 *
	 * @return The value of the knot
	 */
	const double get_F(const std::vector<size_t>& _coord, const size_t _N) const;
	/**
	 * @brief Get the value of one knot of __U
	 *
	 * @param _idx The index of the knot.
	 * @param _N The order of the grid.
	 *
	 * @return The value of the knot.
	 */
	const double get_U(const size_t _idx, const size_t _N) const;
	/**
	 * @brief Get the value of one knot of __F
	 *
	 * @param _idx The index of the knot.
	 * @param _N The order of the grid.
	 *
	 * @return The value of the knot.
	 */
	const double get_F(const size_t _idx, const size_t _N) const;
	/**
	 * @brief Smooth on the given grid.
	 *
	 * @param _M The order of the grid.
	 */
	void smooth(const size_t _M);
	/**
	 * @brief Get the precise solution on the coarest grid.
	 *
	 * @param _M The order of the given grid.
	 */
	void solve_coarest(const size_t _M, const double _e);
	/**
	 * @brief Set the error back to __F and restrict it to coareser grid.
	 *
	 * @param _M The given order of present grid.
	 */
	void set_e(const size_t _M);
	/**
	 * @brief Set the error of v back to __U and interpolate it to finer grid.
	 *
	 * @param _M The given order of present grid.
	 */
	void putback_e(const size_t _M);

	void show_U(const size_t _M) {
		size_t num_1d[2];
		size_t N = get_N();
		size_t Nx = get_Nx();
		size_t Ny = get_Ny();
		num_1d[0] = (1 << Nx - N + _M) + 1;
		num_1d[1] = (1 << Ny - N + _M) + 1;
		std::cout << std::endl << "U:" << std::endl;
		for(size_t i = 0; i != num_1d[1]; i++) {
			for(size_t j = 0; j != num_1d[0]; j++) {
				std::cout << __U(i*num_1d[0] + j, _M) << '\t';
			}
			std::cout << std::endl;
		}
	};
	
	void show_F(const size_t _M) {
		size_t num_1d[2];
		size_t N = get_N();
		size_t Nx = get_Nx();
		size_t Ny = get_Ny();
		num_1d[0] = (1 << Nx - N + _M) + 1;
		num_1d[1] = (1 << Ny - N + _M) + 1;
		std::cout << std::endl << "F:" << std::endl;
		for(size_t i = 0; i != num_1d[1]; i++) {
			for(size_t j = 0; j != num_1d[0]; j++) {
				std::cout << __F(i*num_1d[0] + j, _M) << '\t';
			}
			std::cout << std::endl;
		}
	};
	/**
	 * @brief Solve the poisson equation.
	 *
	 * @param _f The equation function.
	 * @param _g The boundary function.
	 * @param _v The initial guess.
	 * @param _N The order of the finest grid.
	 * @param _M The order of the coarest grid.
	 * @param itr_num The number that we perform iteration.
	 * @param nu1 The number of the frist smooth iteration.
	 * @param nu2 The number of the second smooth iteration.
	 * @param _epsilon The tolerant error.
	 */
	virtual void Solve(std::vector<double>& _v, std::vector<double>& _f,
					   const size_t _Nx, const size_t _Ny, const size_t _M,
					   const size_t itr_num,  const size_t nu1, const size_t nu2, const double _epsilon) = 0;
protected:

	/**
	 * @brief The interpolation operator
	 */
	Interpolation_2d __I_2h_2_h;

	/**
	 * @brief The restriction operator
	 */
	Restriction_2d __I_h_2_2h;
			
	/**
	 * @brief Store the initial guess and the solution will replace it after solving the PDE.
	 */
	MultigridVector_2d __U;

	/**
	 * @brief Store the right side of the equation and residual equation.
	 */
	MultigridVector_2d __F;
};

class FMG_2d : public MultigridSolver_2d {
public:
	FMG_2d() : MultigridSolver_2d(){};
	~FMG_2d(){};
	void Solve(std::vector<double>& _v, std::vector<double>& _f,
			   const size_t _Nx, const size_t _Ny, const size_t _M, const size_t itr_num,
			   const size_t nu1, const size_t nu2, const double _epsilon) override;
	void VC(const size_t _N, const size_t _M, const size_t nu1, const size_t nu2,
			const double _epsilon);
};

MultigridVector_2d::MultigridVector_2d() : std::vector<double>() {
	__Nx = 0;
	__Ny = 0;
	__start_index.clear();
};

MultigridVector_2d::MultigridVector_2d(const vector<double>& _u, const size_t _Nx,
									const size_t _Ny) : std::vector<double>() {
	size_t N;
	if(_Nx < _Ny)
		N = _Nx;
	else
		N = _Ny;
	__Nx = _Nx;
	__Ny = _Ny;
	size_t l = 0;
	__start_index.clear();
	__start_index.resize(N);
	size_t size = 0;
	clear();
	size_t index = 0;
	for(size_t i = 0; i != N; i++) {
		index = index + l;
		__start_index[i] = index;
		l = 1;
		l = l*((1 << _Nx - N + i + 1) + 1)*((1 << _Ny - N + i + 1) + 1);
		size += l;
	}
	resize(size);
	for(size_t i = 0; i != l; i++) {
		(*this)[__start_index[N - 1] + i] = _u[i];
	}
};

void MultigridVector_2d::set(const vector<double>& _u, const size_t _Nx, const size_t _Ny) {
	size_t N;
	if(_Nx < _Ny)
		N = _Nx;
	else
		N = _Ny;
	__Nx = _Nx;
	__Ny = _Ny;
	size_t l = 0;
	__start_index.clear();
	__start_index.resize(N);
	size_t size = 0;
	clear();
	size_t index = 0;
	for(size_t i = 0; i != N; i++) {
		index = index + l;
		__start_index[i] = index;
		l = ((1 << _Nx - N + i + 1) + 1)*((1 << _Ny - N + i + 1) + 1);
		size += l;
	}
	resize(size);
	for(size_t i = 0; i != l; i++) {
		(*this)[__start_index[N - 1] + i] = _u[i];
	}
};

const double& MultigridVector_2d::operator()(const std::vector<size_t>& _coord, const size_t _M) const {
	size_t N = get_N();
	if(_M > N) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	if(_coord.size() != 2) {
		std::cout << "The wrong dimension of the given coordinate." << std::endl;
		std::exit(-1);
	}
	size_t index = __start_index[_M - 1];
	index += _coord[0];
	index += _coord[1]*((1 << __Nx - N + _M) + 1);
	return (*this)[index];
};

double& MultigridVector_2d::operator()(const std::vector<size_t>& _coord, const size_t _M) {
	size_t N = get_N();
	if(_M > N) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	if(_coord.size() != 2) {
		std::cout << "The wrong dimension of the given coordinate." << std::endl;
		std::exit(-1);
	}
	size_t index = __start_index[_M - 1];
	index += _coord[0];
	index += _coord[1]*((1 << __Nx - N + _M) + 1);
	return (*this)[index];
};

const double& MultigridVector_2d::operator()(const size_t index, const size_t _M) const {
	if(_M > get_N() || _M == 0) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	return (*this)[(*this).get_start_index(_M) + index];
};

double& MultigridVector_2d::operator()(const size_t index, const size_t _M) {
	if(_M > get_N() || _M == 0) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	return (*this)[(*this).get_start_index(_M) + index];
};

const size_t MultigridVector_2d::get_N() const {
	size_t Nx = __Nx;
	size_t Ny = __Ny;
	return Nx > Ny ? Ny : Nx;
};

const size_t MultigridVector_2d::get_Nx() const {
	return __Nx;
};

const size_t MultigridVector_2d::get_Ny() const {
	return __Ny;
};

const size_t MultigridVector_2d::get_start_index(const size_t _M) const {
	if(_M > get_N() || _M == 0) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	return __start_index[_M - 1];
};

void MultigridVector_2d::clean(const size_t _M) {
	size_t N = get_N();
	if(_M > get_N() || _M == 0) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	size_t num_x = (1 << __Nx - N + _M) + 1;
	size_t l = 1;
	l = ((1 << __Nx - N + _M) + 1)*((1 << __Ny - N + _M) + 1);
	for(size_t i = 0; i != l; i++) {
		(*this)[__start_index[_M - 1] + i] = 0;
	}
};

void MultigridVector_2d::cleanpart(const size_t _M) {
	std::vector<size_t> coord;
	coord.clear();
	coord.resize(2);
	size_t N = get_N();
	size_t pole = 0;
	size_t num_1d[2];
	num_1d[0] = (1 << __Nx - N + _M) + 1;
	num_1d[1] = (1 << __Ny - N + _M) + 1;
	for(size_t i = 0; i != 2; i++) {
		coord[i] = 1;
	}

	while(pole != 2) {
		pole = 0;
		(*this)(coord, _M) = 0;
		coord[pole]++;
		while(coord[pole] == num_1d[pole] - 1) {
			coord[pole] = 1;
			pole++;
			if(pole == 2)
				break;
			coord[pole]++;
		}
	}
};

void Restriction_2d::operator()(MultigridVector_2d& _v, const size_t _M) {
	size_t Nx = _v.get_Nx();
	size_t Ny = _v.get_Ny();
	size_t N = _v.get_N();
	if(N < _M) {
		std::cout << "The present grids do not lies in the coarest grids" << std::endl;
		std::exit(-1);
	}
	else if(_M <= 1) {
		std::cout << "Can not restrict finer." << std::endl;
		std::exit(-1);
	}
	std::vector<size_t> coord_2h(2, 1);
	std::vector<size_t> coord_h(2, 1);
	std::vector<size_t> d(2, 0);
	size_t pole = 0;
	size_t num_1d[2];
	num_1d[0] = (1 << Nx - N + _M - 1) + 1;
	num_1d[1] = (1 << Ny - N + _M - 1) + 1;
	bool is_out;
	_v.clean(_M - 1);
	double coef = 1;
	//Go through all the nodes in the coareser grid except those on the boundary..
	while(pole != 2) {
		pole = 0;
		is_out = false;
		coord_h[0] = 2*coord_2h[0];
		coord_h[1] = 2*coord_2h[1];
		
		//mid.
		_v(coord_2h, _M - 1) += _v(coord_h, _M);
		if(coord_2h[0] != 0) {
			//west.
			coord_h[0] -= 1;
			_v(coord_2h, _M - 1) += 0.5*_v(coord_h, _M);
			coord_h[0] += 1;
			if(coord_2h[1] != num_1d[1] - 1) {
				//north-west.
				coord_h[0] -= 1;
				coord_h[1] += 1;
				_v(coord_2h, _M - 1) += 0.5*_v(coord_h, _M);
				coord_h[0] += 1;
				coord_h[1] -= 1;
			}
		}
		if(coord_2h[0] != num_1d[0] - 1) {
			//east.
			coord_h[0] += 1;
			_v(coord_2h, _M - 1) += 0.5*_v(coord_h, _M);
			coord_h[0] -= 1;
			if(coord_2h[1] != 0) {
				//south-east.
				coord_h[0] += 1;
				coord_h[1] -= 1;
				_v(coord_2h, _M - 1) += 0.5*_v(coord_h, _M);
				coord_h[0] -= 1;
				coord_h[1] += 1;
			}
		}
		if(coord_2h[1] != 0) {
			//south.
			coord_h[1] -= 1;
			_v(coord_2h, _M - 1) += 0.5*_v(coord_h, _M);
			coord_h[1] += 1;	
		}
		if(coord_2h[1] != num_1d[1] - 1) {
			//north.
			coord_h[1] += 1;
			_v(coord_2h, _M - 1) += 0.5*_v(coord_h, _M);
			coord_h[1] -= 1;
		}

		//next
		coord_2h[pole]++;
		while(coord_2h[pole] == num_1d[pole]) {
			coord_2h[pole] = 0;
			pole++;
			if(pole == 2)
				break;
			coord_2h[pole]++;
		}
	}
};

void Interpolation_2d::operator()(MultigridVector_2d& _v, const size_t _M) {
	if(_M == 0 || _M >= _v.get_N()) {
		std::cout << "Invalid _M" << std::endl;
		std::exit(-1);
	}
	size_t Nx = _v.get_Nx();
	size_t Ny = _v.get_Ny();
	size_t N = _v.get_N();
	std::vector<size_t> coord_2h(2, 0);
	std::vector<size_t> coord_h(2, 0);
	int d[2] = {0};
	size_t pole = 0;
	size_t num_1d[2];
	num_1d[0] = (1 << Nx - N + _M) + 1;
	num_1d[1] = (1 << Ny - N + _M) + 1;
	//clear.
	_v.cleanpart(_M + 1);
	std::vector<double> B(4*num_1d[1] + 4*num_1d[0], 0);
	size_t index = 0;
	for(size_t i = 0; i != num_1d[1]*2 - 1; i++) {
		for(size_t j = 0; j != num_1d[0]*2 - 1; j++) {
			if(i == 0 || i == num_1d[1]*2 - 2 || j == 0 || j == num_1d[0]*2 - 2) {
				B[index] = _v(i*(num_1d[0]*2 - 1) + j, _M + 1);
				index++;
			}
		}
	}

	//Go through all the nodes in the coareser grid.
	while(pole != 2) {
		coord_h[0] = 2*coord_2h[0];
		coord_h[1] = 2*coord_2h[1];
		_v(coord_h, _M + 1) += _v(coord_2h, _M);
		if(coord_2h[0] != 0) {
			//west.
			coord_h[0] -= 1;
			_v(coord_h, _M + 1) += 0.5*_v(coord_2h, _M);
			coord_h[0] += 1;
			if(coord_2h[1] != num_1d[1] - 1) {
				//north-west.
				coord_h[0] -= 1;
				coord_h[1] += 1;
				_v(coord_h, _M + 1) += 0.5*_v(coord_2h, _M);
				coord_h[0] += 1;
				coord_h[1] -= 1;
			}
		}
		if(coord_2h[0] != num_1d[0] - 1) {
			//east.
			coord_h[0] += 1;
			_v(coord_h, _M + 1) += 0.5*_v(coord_2h, _M);
			coord_h[0] -= 1;
			if(coord_2h[1] != 0) {
				//south-east.
				coord_h[0] += 1;
				coord_h[1] -= 1;
				_v(coord_h, _M + 1) += 0.5*_v(coord_2h, _M);
				coord_h[0] -= 1;
				coord_h[1] += 1;
			}
		}
		if(coord_2h[1] != 0) {
			//south.
			coord_h[1] -= 1;
			_v(coord_h, _M + 1) += 0.5*_v(coord_2h, _M);
			coord_h[1] += 1;	
		}
		if(coord_2h[1] != num_1d[1] - 1) {
			//north.
			coord_h[1] += 1;
			_v(coord_h, _M + 1) += 0.5*_v(coord_2h, _M);
			coord_h[1] -= 1;
		}
		
		pole = 0;
		coord_2h[pole]++;
		while(coord_2h[pole] == num_1d[pole]) {
			coord_2h[pole] = 0;
			pole++;
			if(pole == 2)
				break;
			coord_2h[pole]++;
		}
	}
	index = 0;
	for(size_t i = 0; i != num_1d[1]*2 - 1; i++) {
		for(size_t j = 0; j != num_1d[0]*2 - 1; j++) {
			if(i == 0 || i == num_1d[1]*2 - 2 || j == 0 || j == num_1d[0]*2 - 2) {
				_v(i*(num_1d[0]*2 - 1) + j, _M + 1) = B[index];
				index++;
			}
		}
	}
};

MultigridSolver_2d::MultigridSolver_2d() {
	__F.clear();
	__U.clear();
};

void MultigridSolver_2d::set(const std::vector<double>& _v, const size_t _Nx, const size_t _Ny,
							 const std::vector<double>& _f) {
	size_t l = 1;
	l = ((1 << _Nx) + 1)*((1 << _Ny) + 1);
	if(_v.size() != l) {
		std::cout << "The length of the initial guess does not meet with the grid!" << std::endl;
	}
	if(_f.size() != l) {
		std::cout << "The length of the RHS does not meet with the grid!" << std::endl;
	}
	__U.set(_v, _Nx, _Ny);
	__F.set(_f, _Nx, _Ny);
};

void MultigridSolver_2d::omit_e(const double _epsilon) {
	size_t l = 1;
	size_t N = get_N();
	size_t Nx = get_Nx();
	size_t Ny = get_Ny();
	size_t num_1d[2];
	num_1d[0] = (1 << Nx) + 1;
	num_1d[1] = (1 << Ny) + 1;
	for(size_t i = 0; i != num_1d[0]*num_1d[1]; i++) {
		if(fabs(__U(i, N)) < _epsilon)
			__U(i, N) = 0;
		if(fabs(__F(i, N)) < _epsilon)
			__F(i, N) = 0;
	}
};
const size_t MultigridSolver_2d::get_N() const {
	return __U.get_N();
};

const size_t MultigridSolver_2d::get_Nx() const {
	return __U.get_Nx();
};

const size_t MultigridSolver_2d::get_Ny() const {
	return __U.get_Ny();
};

const double MultigridSolver_2d::get_U(const std::vector<size_t>& _coord, const size_t _M) const {
	return __U(_coord, _M);
};

const double MultigridSolver_2d::get_F(const std::vector<size_t>& _coord, const size_t _M) const {
	return __F(_coord, _M);
};

const double MultigridSolver_2d::get_U(const size_t _idx, const size_t _M) const {
	return __U(_idx, _M);
};

const double MultigridSolver_2d::get_F(const size_t _idx, const size_t _M) const {
	return __F(_idx, _M);
};

void MultigridSolver_2d::smooth(const size_t _M) {
	size_t l = 1;
	size_t N = get_N();
	size_t Nx = get_Nx();
	size_t Ny = get_Ny();
	if(_M > N || _M == 0) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	size_t num_1d[2];
	num_1d[0] = (1 << Nx - N + _M) + 1;
	num_1d[1] = (1 << Ny - N + _M) + 1;
	l = (num_1d[0] - 2)*(num_1d[1] - 2);

	std::vector<size_t> coord(2, 1);
	double coef = 1.0;
	std::vector<double> v(l, 0);
	size_t pre_index;
	bool is_stop = false;
	size_t pole = 0;
	pre_index = 0;
	while(!is_stop) {
		pole = 0;
		//west.
		coord[0] -= 1;
		v[pre_index] += 1.0*(1 << Nx)/(1 << Ny)*__U(coord, _M);
		coord[0] += 1;
		//east.
		coord[0] += 1;
		v[pre_index] += 1.0*(1 << Nx)/(1 << Ny)*__U(coord, _M);
		coord[0] -= 1;
		//south.
		coord[1] -= 1;
		v[pre_index] += 1.0*(1 << Ny)/(1 << Nx)*__U(coord, _M);
		coord[1] += 1;
		//north.
		coord[1] += 1;	
		v[pre_index] += 1.0*(1 << Ny)/(1 << Nx)*__U(coord, _M);
		coord[1] -= 1;
		coord[pole]++;
		pre_index++;
		while(coord[pole] == num_1d[pole] - 1) {
			coord[pole] = 1;
			pole++;
			if(pole == 2)
				break;
			coord[pole]++;
		}
		if(pole == 2)
			is_stop = true;
	}
	is_stop = false;
	pre_index = 0;
	while(!is_stop) {
		pole = 0;
		__U(coord, _M) = 1.0/(2.0*(1 << Nx)/(1 << Ny) 
							+ 2.0*(1 << Ny)/(1 << Nx))*(v[pre_index] + __F(coord, _M));
		coord[pole]++;
		pre_index++;
		while(coord[pole] == num_1d[pole] - 1) {
			coord[pole] = 1;
			pole++;
			if(pole == 2)
				break;
			coord[pole]++;
		}
		if(pole == 2)
			is_stop = true;
	}
//	std::cout << "Smooth:" << std::endl;
//	this->show_U(_M);
//	this->show_F(_M);
};

void MultigridSolver_2d::solve_coarest(const size_t _M, const double _e) {
//	std::cout << "Solvecoaret." << std::endl;
	size_t l = 1;
	size_t N = get_N();
	size_t Nx = get_Nx();
	size_t Ny = get_Ny();
	if(_M > N || _M == 0) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	size_t num_1d[2];
	num_1d[0] = (1 << Nx - N + _M) + 1;
	num_1d[1] = (1 << Ny - N + _M) + 1;
	l = (num_1d[0] - 2)*(num_1d[1] - 2);
	double A[l*l] = {0};
	double v[l] = {0};
	size_t pre_index = 0;

	for(size_t i = 1; i != num_1d[1] - 1; i++) {
		for(size_t j = 1; j != num_1d[0] - 1; j++) {
			v[pre_index] += __F(i*num_1d[0] + j, _M);
			if(j == 1) 
				v[pre_index] += __U(i*num_1d[0] + j - 1, _M)*(1 << Nx)/(1 << Ny);
			else if(j == num_1d[0] - 2)
				v[pre_index] += __U(i*num_1d[0] + j + 1, _M)*(1 << Nx)/(1 << Ny);
			if(i == 1) 
				v[pre_index] += __U((i - 1)*num_1d[0] + j, _M)*(1 << Ny)/(1 << Nx);
			else if(i == num_1d[1] - 2)
				v[pre_index] += __U((i + 1)*num_1d[0] + j, _M)*(1 << Ny)/(1 << Nx);
			if(j != 1)
				//west.
				A[pre_index*l + pre_index - 1] += -1.0*(1 << Nx)/(1 << Ny);
			
			if(j != num_1d[0] - 2)
				//east.
				A[pre_index*l + pre_index + 1] += -1.0*(1 << Nx)/(1 << Ny);
			
			if(i != 1)
				//south.
				A[pre_index*l + pre_index - num_1d[0] + 2] += -1.0*(1 << Ny)/(1 << Nx);
			
			if(i != num_1d[1] - 2)
				//north.
				A[pre_index*l + pre_index + num_1d[0] - 2] += -1.0*(1 << Ny)/(1 << Nx);
			
			//mid.
			A[pre_index*l + pre_index] += 2.0*(1 << Nx)/(1 << Ny)
										  + 2.0*(1 << Ny)/(1 << Nx);
			pre_index++;
		}
	}

	int ipiv[l];
	LAPACKE_dgesv(LAPACK_COL_MAJOR, l, 1, A, l, ipiv, v, l);

	std::vector<size_t> coord(2, 0);
	pre_index = 0;
	for(size_t i = 0; i != num_1d[1] - 2; i++) {
		for(size_t j = 0; j != num_1d[0] - 2; j++) {
			coord[0] = j + 1;
			coord[1] = i + 1;
			__U(coord, _M) = v[pre_index];
			pre_index++;
		}
	}
//	this->show_U(_M);
};

void MultigridSolver_2d::set_e(const size_t _M) {
	size_t l = 1;
	size_t N = get_N();
	size_t Nx = get_Nx();
	size_t Ny = get_Ny();
	if(_M > N || _M <= 1) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	size_t num_1d[2];	
	num_1d[0] = (1 << Nx - N + _M) + 1;
	num_1d[1] = (1 << Ny - N + _M) + 1;
	l = (num_1d[0])*(num_1d[1]);
	std::vector<double> F(l, 0);
	size_t pre_index = 0;
	for(size_t i = 0; i != l; i++) {
		F[i] = __F(i, _M);
	}
	__F.clean(_M);
	for(size_t i = 1; i != num_1d[1] - 1; i++) {
		for(size_t j = 1; j != num_1d[0] - 1; j++) {
			pre_index = i*num_1d[0] + j;
			__F(pre_index, _M) = F[pre_index];
			__F(pre_index, _M) -= 2.0*(1 << Ny)/(1 << Nx)*__U(pre_index, _M)
							  + 2.0*(1 << Nx)/(1 << Ny)*__U(pre_index, _M);
			__F(pre_index, _M) -= -1.0*(1 << Nx)/(1 << Ny)
								  *(__U(pre_index - 1, _M) + __U(pre_index + 1, _M));
			__F(pre_index, _M) -= -1.0*(1 << Ny)/(1 << Nx)
								  *(__U(pre_index - num_1d[0], _M) + __U(pre_index + num_1d[0], _M));
		}
	}
//	for(size_t i = 0; i != num_1d[1]; i++) {
//		for(size_t j = 0; j != num_1d[0]; j++) {
//			if(i == 0 || i == num_1d[1] - 1 || j == 0 || j == num_1d[0] - 1) {
//				__F(i*num_1d[0] + j, _M) = 0;
//			}
//		}
//	}
//	std::cout << "Befor restric F:" << std::endl;
//	this->show_F(_M);
//	this->show_F(_M - 1);
	__I_h_2_2h(__F, _M);
//	std::cout << "After" << std::endl;
//	this->show_F(_M - 1);
	for(size_t i = 0; i != l; i++)
		__F(i, _M) = F[i];
//	std::cout << "Set_e:" << std::endl;
//	this->show_F(_M - 1);
};

void MultigridSolver_2d::putback_e(const size_t _M) {
	size_t l = 1;
	size_t N = get_N();
	size_t Nx = get_Nx();
	size_t Ny = get_Ny();
	if(_M > N - 1 || _M == 0) {
		std::cout << "Out of range!" << std::endl;
		std::exit(-1);
	}
	size_t num_1d[2];	
	num_1d[0] = (1 << Nx - N + _M + 1) + 1;
	num_1d[1] = (1 << Ny - N + _M + 1) + 1;
	l = (num_1d[0])*(num_1d[1]);
	std::vector<size_t> coord(2, 0);
	std::vector<double> v(l, 0);
	size_t pre_index = 0;
	for(size_t i = 0; i != l; i++) 
		v[i] = __U(i, _M + 1);
//	std::cout << "Before inter U:" << std::endl;
//	this->show_U(_M);
//	this->show_U(_M + 1);
	__I_2h_2_h(__U, _M);
//	std::cout << "After" << std::endl;
//	this->show_U(_M + 1);


	for(size_t i = 0; i != num_1d[1]; i++) {
		for(size_t j = 0; j != num_1d[0]; j++) {
			pre_index = i*num_1d[0] + j;
			__U(pre_index, _M + 1) += v[pre_index];
		}
	}
//	std::cout << "putback_e" << std::endl;
//	this->show_U(_M + 1);
};

void FMG_2d::Solve(std::vector<double>& _v, std::vector<double>& _f,
			   const size_t _Nx, const size_t _Ny, const size_t _M, const size_t itr_num,
			   const size_t nu1, const size_t nu2, const double _epsilon) {
	bool is_stop = true;
	this->set(_v, _Nx, _Ny, _f);
	//this->omit_e(_epsilon);
	size_t N = this->get_N();
	size_t pre_N = N;
	size_t num_1d[2];	
	size_t index = 0;
	std::vector<size_t> coord_h(2, 0);
	std::vector<size_t> coord_2h(2, 0);
	double coef = 1.0;
	for(size_t i = N - 1; i != _M - 1; i--) {
		(this->__I_h_2_2h)(this->__F, pre_N);
		for(size_t k = 0; k != 2; k++) {
			for(size_t j = 0; j != num_1d[0]; j++) {
				coord_2h[0] = j;
				coord_2h[1] = k*(num_1d[1] - 1);
				coord_h[0] = coord_2h[0]*2;
				coord_h[1] = coord_2h[1]*2;
				__U(coord_2h, i) = __U(coord_h, i + 1);
			}
		}
		for(size_t k = 0; k != num_1d[1]; k++) {
			for(size_t j = 0; j != 2; j++) {
				coord_2h[0] = j*(num_1d[0] - 1);
				coord_2h[1] = k;
				coord_h[0] = coord_2h[0]*2;
				coord_h[1] = coord_2h[1]*2;
				__U(coord_2h, i) = __U(coord_h, i + 1);
			}
		}
	}
//	this->show_U(pre_N);
//	this->show_F(pre_N);
	pre_N = _M;	
	this->VC(pre_N, _M, nu1, nu2, _epsilon);
//	std::cout << "After VC" << std::endl;
//	this->show_U(pre_N);
//	this->show_F(pre_N);
	if(N > _M)
		is_stop = false;
	while(!is_stop) {
//		std::cout << "Before int:";
//		this->show_U(pre_N);
		(this->__I_2h_2_h)(this->__U, pre_N);
//		std::cout << "Inter:" << std::endl;
//		this->show_U(pre_N + 1);
		this->__U.clean(pre_N - 1);
		pre_N++;
		for(size_t i = 0; i != itr_num; i++) {
//			std::cout << "Before VC:" << std::endl;
//			this->show_U(pre_N);
			this->VC(pre_N, _M, nu1, nu2, _epsilon);
//			std::cout << "After VC:" << std::endl;
//			this->show_U(pre_N);
		}
		if(pre_N == N)
			is_stop = true;
	}
};

void FMG_2d::VC(const size_t _N, const size_t _M, const size_t nu1, const size_t nu2, const double _epsilon) {
	bool is_stop = false;
	size_t pre_N = _N;
//	std::cout << "Start N:" << pre_N  << '\t' << _M << std::endl;
	size_t l = 1;
	size_t N = this->get_N();
	size_t Nx = this->get_Nx();
	size_t Ny = this->get_Ny();
	size_t num_1d[2];
	while(!is_stop) {
		for(size_t i = 0; i != nu1; i++) {
//			std::cout << "before smooth" << std::endl;
//			this->show_U(pre_N);
//			this->show_F(pre_N);
			this->smooth(pre_N);
//			this->show_U(pre_N);
		}
		if(pre_N == _M) {
			is_stop = true;
			break;
		}
		this->__F.clean(pre_N - 1);
//		std::cout << "before set_e";
//		this->show_U(pre_N);
//		this->show_F(pre_N);
		this->set_e(pre_N);
//		this->show_F(pre_N - 1);
		pre_N = pre_N - 1;
		num_1d[0] = (1 << Nx - N + pre_N) + 1;
		num_1d[1] = (1 << Ny - N + pre_N) + 1;
		std::vector<size_t> coord(2, 1);
		size_t pole = 0;
		this->__U.clean(pre_N);
	}
//	std::cout << "ad:" << pre_N << std::endl;
	if(pre_N == _M)	
		this->solve_coarest(pre_N, _epsilon);
//	std::cout << "After solve";
//	this->show_U(pre_N);
	if(_N > _M)
		is_stop = false;
	while(!is_stop) {
		this->putback_e(pre_N);
		pre_N++;
		for(size_t i = 0; i != nu2; i++) {
//			std::cout << "Before smooth";
//				this->show_U(pre_N);
//			this->show_F(pre_N);
			this->smooth(pre_N);
//			this->show_U(pre_N);
		}
		if(pre_N == _N) {
			is_stop = true;
		}
	}
};

#else
//Do nothing.
#endif
